#include <iostream>
#include <cstdlib>
#include <cstdint>
#include "jps.hpp"
#include "tools.hpp"
#include <limits>

using namespace std;

#define FORCE_MASK(B) ((B >> 1) & (~B))

Location quickSearchHorizontal(const Grid& grid, const Location node, const Location dir, const Location goal)
{
	uint32_t blockN = grid.fetchSegBlock(node, dir);
	/* 如果上下移动就变成了要取左右，dir的x和y必定有一个是0
		假设是x~=0，左右移动，那么要取的是y - 1和y + 1
		假设是y~=0，上下移动，那么要取的是x - 1和x + 1
		up可能是down，down可能是up
	*/
	const Location inverted_dir {dir.y, dir.x};
	Location node_up = node + inverted_dir;
	Location node_down = node - inverted_dir;
	uint32_t blockUp = grid.fetchSegBlock(node_up, dir);
	uint32_t blockDown = grid.fetchSegBlock(node_down, dir);
	uint32_t blockStop = FORCE_MASK(blockUp) | FORCE_MASK(blockDown) | blockN;

	// 考虑提前撞到终点的问题
	// 我们计算node -> 终点的方向变量，然后判断是不是和当前方向为同向
	// 如果不是，后续逻辑完全不用考虑终点问题；如果是，且方向是直线方向，才需要考虑

	// block段中完全没阻挡，有可能这段里有终点
	// block段中有stop点，得判断终点在bitStop的之前还是在之后

	// 如果终点不在这个方向上，或者不在这一段block上，那么blockGoal == 0
	// 否则，我们将终点对应的bit位置1
	// 然后取bitGoal，终点前的前导0数量，拿它和bitStop进行比较
	uint32_t bitGoal = 0;
	if (node.x == goal.x || node.y == goal.y) {
		Location toGoalDist = (goal - node);
		Location toGoalDir = toGoalDist.direction();
		if (dir == toGoalDir) {
			uint32_t abs_dist_x = std::abs(toGoalDist.x);
			uint32_t abs_dist_y = std::abs(toGoalDist.y);
			// 在本次探索的段内
			if ((dir.x != 0 && abs_dist_x < FETCH_BLOCK_LEN) || (dir.y != 0 && abs_dist_y < FETCH_BLOCK_LEN)) {
				bitGoal = abs_dist_x > 0 ? abs_dist_x : abs_dist_y;
			}
		}
	}

	// 有至少1个stop点
	if (blockStop != 0) {
		// 计算前导0个数
		uint32_t bitStop = __builtin_clz(blockStop);
		uint32_t bitN = (blockN != 0) ? __builtin_clz(blockN) : FETCH_BLOCK_LEN;
		printf("N(%d, %d), dir(%d, %d), %d|%d|%d|\n", node.x, node.y, dir.x, dir.y, bitGoal, bitStop, bitN);

		// bitGoal < bitStop很好懂，终点在段内的位置居于段内碰到的第一个跳点之前，所以返回终点作为后继

		// bitGoal == bitStop时也返回终点作为后继，为什么呢？
		// bitStop对应的位置是个跳点，而bitStop的后一个位置上才是终点，因为它们同向且相邻，二者之间必定最近且可达
		// 所以我认为可以直接省略掉bitStop对应的跳点，直接返回终点作为后继
		// 当然了，这样的改动和jps算法的定义不太一样，毕竟它要求碰到第一个跳点时就停下来并返回

		/* 具体的，你可以根据下面这个测试用例，看一下(bitGoal < bitStop)和(bitGoal <= bitStop)的区别
			unordered_set<Location> wall_locs {
				{1, 1},
				{2, 2}, {2, 3}, {2, 4}, {2, 5}, {2, 6}, {2, 7},
				{4, 5},
				{5, 0}, {5, 1}, {5, 2}, {5, 3}, {5, 4}, {5, 5},
				{6, 5},
				{7, 2}, {7, 3}, {7, 5},
			};
			Grid map {10, 10, wall_locs};
			Location start {1, 2};
			Location goal {6, 1};
		*/
		if (bitGoal > 0 && bitGoal <= bitStop) {
			return goal;
		}

		if (bitStop < bitN) {
			// bitStop对应的位置是个跳点，因为N本身就是block的第一个数值高位的bit位，所以bitStop - 1才是基于N所需要递进的步长
			return node + (dir * (bitStop - 1));
		} else {
			// 这个方向上的探索会碰到障碍物
			return NoneLoc;
		}
	} else {
		printf("N(%d, %d), dir(%d, %d), %d|%d|%d\n", node.x, node.y, dir.x, dir.y, bitGoal, 0, 0);
		if (bitGoal > 0) {
			return goal;
		}

		// N沿着当前的直线方向，向前跃迁N_JUMP_LEN个格子
		return quickSearchHorizontal(grid, node + (dir * N_JUMP_LEN), dir, goal);
	}

	// 作者读block是直接读一片的，他是怎么操作的呢？我们不应该一个个地去读吧，感觉效率有点差的
	// 作者写的例子是直接死循环找stop点，然后判断终点会不会在更前的位置上
}

// jump()表示initial + dir的一次格子递进，初始initial就是起点，dir是它延伸出来的方向
// quickSearchHorizontal()表示以N为基点，在水平方向dir上进行的jps-bit搜索
Location jump(const Grid& grid, const Location initial, const Location dir, const Location goal)
{
	// jump()递进到下个格子，必须判断是否可通行
	if(!grid.valid_move(initial, dir)){
		return NoneLoc;
	}
	// 若递进到了终点，直接返回
	auto new_loc {initial + dir};
	if(new_loc == goal){
		return new_loc;
	}

	// 这里之前得判断是否有强制邻居
	// 现在我们把对强制邻居的判断扔到了quickSearchHorizontal()里
	printf("jump to (%d, %d), dir (%d, %d)\n", new_loc.x, new_loc.y, dir.x, dir.y);

	if(dir.x != 0 && dir.y !=0){
		// 当前探索方向为斜向，则将斜向拆分成2个直线方向，以new_loc = N，进行jps-bit探索
		for(const auto& new_dir: {Location{dir.x, 0}, Location{0, dir.y}}){
			auto jump_point {quickSearchHorizontal(grid, new_loc, new_dir, goal)};
			if(jump_point != NoneLoc){
				return new_loc;
			}
		}
		// 拆分出来的2个直线方向探索完了之后，沿当前斜向继续前进
		return jump(grid, new_loc, dir, goal);
	} else {
		// 直线方向可直接进行jps-bit探索
		return quickSearchHorizontal(grid, new_loc, dir, goal);
	}
}

vector<Location> successors(const Grid& grid, const Location& current, const Location& parent, const Location& goal)
{
	printf("---------------- base on (%d, %d), parent (%d, %d) ----------------\n", current.x, current.y, parent.x, parent.y);

	vector<Location> successors;
	// 遍历自然邻居、强制邻居，这一步已经把不符合walk_able的邻居全部删去
	auto neighbours {grid.pruned_neighbours(current, parent)};
	for(const auto& n: neighbours){
		// 沿current -> n的方向执行jump()函数
		auto jump_point {jump(grid, current, (n - current).direction(), goal)};
		// 找到跳点了就加入successors
		if(jump_point != NoneLoc){
			printf("collect jumpNode(%d, %d)\n", jump_point.x, jump_point.y);
			successors.push_back(jump_point);
		}
	}
	return successors;
}

unordered_map<Location, Location> jps(const Grid& grid, const Location& start, const Location& goal, heuristic_fn heuristic)
{
	PQLoc open_set;
	unordered_map<Location, Location> came_from {}; // 我们用1个unordered_map去缓存节点之间的parent关系，key为subLoc，value为parent
	unordered_map<Location, double> cost_so_far {}; // cost_so_far缓存的是loc obj的g值

	// 起点加入小根堆，f值 = 0
	open_set.emplace(0, start);
	// 起点g值 = 0
	cost_so_far[start] = 0;
	// 起点无parent
	came_from[start] = NoneLoc;

	// open_set不为空就继续
	while(!open_set.empty()){
		// 小根堆open_set弹出f值最小的loc obj；若找到了终点，提前结束寻路过程
		const auto current {open_set.top().second};
		if(current == goal){
			break;
		}
		open_set.pop();

		Location parent = came_from[current];

		// 方向 = parent -> current，寻找基点current的后继者们
		for(const auto& next : successors(grid, current, parent, goal)){
			// 以勾股定理计算新的g值 = current的累计g值 + current到next的距离
			const auto new_cost = cost_so_far[current] + heuristic(current, next);
			// next在cost_so_far中作为key出现的次数，返回0或1
			unsigned int existing_count = cost_so_far.count(next);
			// 先把existing_cost初始化为double类型的最大值
			auto existing_cost = std::numeric_limits<double>::max();
			// 如果先前计算过跳点next的g值了，则赋予existing_cost
			if (existing_count) {
				existing_cost = cost_so_far.at(next);
			}
			// 若跳点next无g值，或新的g值更小，则更新跳点next的g、h、f值、父节点，并加入openset
			if (existing_count == 0 || new_cost < existing_cost) {
				cost_so_far[next] = new_cost;
				came_from[next] = current;
				// 在优先队列中直接构造并插入1个元素，f = g + h
				double fvalue = new_cost + heuristic(next, goal);
				open_set.emplace(fvalue, next);

				printf("openset add jumpNode(%d, %d) exist_count=%d, fvalue=%.3f, new_gvalue=%.3f\n", next.x, next.y, existing_count, fvalue, new_cost);
			}
		}
	}
	return came_from;
}


int main()
{
	// unordered_set<Location> wall_locs {
	// 	{1, 1},
	// 	{2, 3}, {2, 4}, {2, 5}, {2, 6}, {2, 7},
	// 	{4, 5},
	// 	{5, 0}, {5, 1}, {5, 2}, {5, 3}, {5, 4}, {5, 5},
	// 	{6, 5},
	// 	{7, 2}, {7, 3}, {7, 5},
	// };
	// Grid map {10, 10, wall_locs};

	// Location start {1, 2};
	// Location goal {6, 1};

	unordered_set<Location> wall_locs {
		{1, 2}, {2, 2}, {3, 2}, {4, 2},
		{1, 3}, 		{3, 3},
		{1, 4}, 		{3, 4},
		{1, 5},
		{1, 6}, {2, 6}, {3, 6}, {4, 6},
	};
	Grid map {10, 10, wall_locs};

	Location start {0, 3};
	Location goal {2, 4};

	// unordered_set<Location> wall_locs {
	// 	{1, 1},
	// 	{2, 2}, {2, 3}, {2, 4}, {2, 5}, {2, 6}, {2, 7},
	// 	{4, 5},
	// 	{5, 0}, {5, 1}, {5, 2}, {5, 3}, {5, 4}, {5, 5},
	// 	{6, 5},
	// 	{7, 2}, {7, 3}, {7, 5},
	// };
	// Grid map {60, 40, wall_locs};

	// Location start {0, 38};
	// Location goal {59, 1};

	// 开始寻路，h(n)用的是欧里几得距离
	auto came_from = jps(map, start, goal, Tool::euclidean);

	// 处理并得出找到的通路
	auto path = Tool::reconstruct_path(start, goal, came_from);
	// 打印通路
	Tool::draw_grid(map, {}, {}, path, came_from, start, goal);
}
